// center_detector.cpp
#include "center_detection/center_detector.hpp"
#include "center_detection/center_json.hpp"
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <geometry_msgs/msg/point_stamped.hpp>
#include <std_msgs/msg/string.hpp>
#include <mutex>

// 包含 LifecycleNode 相关头文件
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "rclcpp_lifecycle/lifecycle_publisher.hpp"
#include "lifecycle_msgs/msg/transition.hpp"

namespace center_detection {

    // 构造函数现在继承自 LifecycleNode
    CenterDetector::CenterDetector() : rclcpp_lifecycle::LifecycleNode("center_detector") {
        RCLCPP_INFO(this->get_logger(), "Lifecycle source center detector is now created.");
        // 不再在此处初始化，因为生命周期节点的初始化由状态机驱动
    }

    // 声明参数
    void CenterDetector::declare_parameters(){
        this->declare_parameter("neutron.search_start_z", 0.6);
        this->declare_parameter("neutron.search_end_z", 0.4);
        this->declare_parameter("neutron.search_step", 0.01);
        this->declare_parameter("neutron.min_points_in_slice", 30);
        this->declare_parameter("neutron.neutron_height", 0.2);
        this->declare_parameter("neutron.neutron_radius", 0.2);
        this->declare_parameter("neutron.fixed_point_a_x", 3.8);
        this->declare_parameter("neutron.fixed_point_a_y", 3.8);

        this->declare_parameter("gamma.search_start_z", 0.35);
        this->declare_parameter("gamma.search_end_z", 0.2);
        this->declare_parameter("gamma.search_step", 0.01);
        this->declare_parameter("gamma.min_points_in_slice", 50);
        this->declare_parameter("gamma.gamma_height", 0.2);
        this->declare_parameter("gamma.top_width", 0.2);
        this->declare_parameter("gamma.top_height", 0.2);
        this->declare_parameter("gamma.width_difference_threshold", 0.2);
        this->declare_parameter("gamma.height_difference_threshold", 0.2);

        this->declare_parameter("subscribing_cloud", "pointcloud_topic");
        this->declare_parameter("publishing_topic", "center_json");
        this->declare_parameter("detection_mode", "neutron"); // 默认值
        this->declare_parameter("alpha", 0.3);
    }

    // 初始化组件
    void CenterDetector::init_components(){
        // 初始化监测管理器
        detection_manager_ = std::make_unique<DetectionManager>();

        // 创建桶检测器
        auto neutron_detector = std::make_shared<NeutronDetector>();
        detection_manager_->registerDetector("neutron", neutron_detector);

        // 创建锥形放射源检测器
        auto gamma_detector = std::make_shared<GammaDetector>();
        detection_manager_->registerDetector("gamma", gamma_detector);
    }

    // 设置 ROS 接口（订阅者、发布者等）
    void CenterDetector::setup_ros_interface(){
        // 获取参数
        subscribing_cloud_ = this->get_parameter("subscribing_cloud").as_string();
        publishing_topic_ = this->get_parameter("publishing_topic").as_string();
        detection_mode_ = this->get_parameter("detection_mode").as_string();
        alpha_ = this->get_parameter("alpha").as_double();

        // 创建订阅者和发布者
        cloud_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
            subscribing_cloud_, 10,
            std::bind(&CenterDetector::cloud_callback, this, std::placeholders::_1));

        // 创建检测模式话题订阅者
        /* detection_mode_sub_ = this->create_subscription<std_msgs::msg::String>(
            "detection_mode", 10, std::bind(&CenterDetector::detection_mode_callback, this, std::placeholders::_1)); */

        // 使用 LifecyclePublisher 替代普通 Publisher
        pub_neutron_center_ = this->create_publisher<geometry_msgs::msg::PointStamped>("/neutron_center", 10);
        pub_gamma_center_ = this->create_publisher<geometry_msgs::msg::PointStamped>("/gamma_center", 10);
        pub_center_json_ = this->create_publisher<std_msgs::msg::String>(publishing_topic_, 10);

        // 设置检测器参数
        auto neutron_detector = std::dynamic_pointer_cast<NeutronDetector>(
            detection_manager_->getDetector("neutron"));
        if (neutron_detector) {
            neutron_detector->setParameters(this);
        }

        auto gamma_detector = std::dynamic_pointer_cast<GammaDetector>(
            detection_manager_->getDetector("gamma"));
        if (gamma_detector) {
            gamma_detector->setParameters(this);
        }
    }


    // --- 生命周期回调函数 ---
   CallbackReturn CenterDetector::on_configure(const rclcpp_lifecycle::State & state)
    {
        RCLCPP_INFO(get_logger(), "Configuring CenterDetector from state: %s", state.label().c_str());

        // 1. 声明参数
        declare_parameters();

        // 2. 初始化组件
        init_components();

        // 3. 设置 ROS 接口 (创建订阅者/发布者对象，但不激活)
        setup_ros_interface();

        // 4. 初始化状态标志
        state_is_active_ = false; // 初始为非活动状态

        // 5. 重置滤波器状态变量
        last_neutron_point_ = geometry_msgs::msg::Point(); // 重置为0
        last_gamma_point_ = geometry_msgs::msg::Point();   // 重置为0
        detection_mode_ = this->get_parameter("detection_mode").as_string(); // 重新获取默认模式或已设置的模式

        RCLCPP_INFO(this->get_logger(), "Successfully configured.");
        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
    }

    CallbackReturn CenterDetector::on_activate(const rclcpp_lifecycle::State & state)
    {
        RCLCPP_INFO(get_logger(), "Activating CenterDetector from state: %s", state.label().c_str());

        // 激活所有发布者和订阅者
        pub_neutron_center_->on_activate();
        pub_gamma_center_->on_activate();
        pub_center_json_->on_activate();
        // cloud_sub_ 没有 on_activate 方法，但其回调现在可以被调用

        state_is_active_ = true; // 标记为活动状态

        RCLCPP_INFO(this->get_logger(), "Successfully activated.");
        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
    }

    CallbackReturn CenterDetector::on_deactivate(const rclcpp_lifecycle::State & state)
    {
        RCLCPP_INFO(get_logger(), "Deactivating CenterDetector from state: %s", state.label().c_str());
        
        // 停用所有发布者和订阅者
        pub_neutron_center_->on_deactivate();
        pub_gamma_center_->on_deactivate();
        pub_center_json_->on_deactivate();

        state_is_active_ = false; // 标记为非活动状态

        RCLCPP_INFO(this->get_logger(), "Successfully deactivated.");
        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
    }

    CallbackReturn CenterDetector::on_cleanup(const rclcpp_lifecycle::State & state)
    {
        RCLCPP_INFO(get_logger(), "Cleaning up CenterDetector from state: %s", state.label().c_str());

        
            // 1. 重置状态标志
            state_is_active_ = false;

            // 2. 重置内部状态变量
            last_neutron_point_ = geometry_msgs::msg::Point();
            last_gamma_point_ = geometry_msgs::msg::Point();
            // detection_mode_ = "neutron"; // 可选：重置为默认模式

            // 3. 清理组件 (如果需要)
            detection_manager_.reset();
            
            // 4. 销毁或重置订阅者/发布者 (LifecyclePublisher 会自动处理)
            // 注意：通常不需要手动销毁，由生命周期管理
            // cloud_sub_.reset(); pub_neutron_center_.reset(); ... // 通常不推荐手动 reset

            RCLCPP_INFO(this->get_logger(), "Successfully cleaned up.");
            return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
    }


    // --- 原有回调函数修改 ---

    /*   
    // 放射源类型回调函数
    void CenterDetector::detection_mode_callback(const std_msgs::msg::String::SharedPtr msg)
    {
        std::lock_guard<std::mutex> lock(detection_mode_mutex_);
        detection_mode_ = msg->data;
        RCLCPP_INFO(this->get_logger(), "Received detection mode from state machine: %s", 
                   detection_mode_.c_str()); 
    } 
    */

    void CenterDetector::cloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) {
        // 如果节点未激活，则不处理数据
        if (!state_is_active_) {
            RCLCPP_DEBUG(this->get_logger(), "Node is not active, skipping cloud callback.");
            return;
        }

        // 转换为PCL节点
        auto laser_cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZI>>();
        pcl::fromROSMsg(*msg, *laser_cloud);
        if(laser_cloud->empty()) {
            return;
        }

        // 确定对应放射源检测器进行中心点识别
        perform_detection(laser_cloud);
    }

    // 其他函数 (perform_detection, applyExponentialFilter) 保持不变
    void CenterDetector::perform_detection(const pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud) {
        // 使用从状态机获取的检测模式
        // 使用互斥锁锁定当前检测模式线程
        std::string current_mode;
        {
            std::lock_guard<std::mutex> lock(detection_mode_mutex_);
            current_mode = detection_mode_;
        }

        // 仅使用桶检测
        if(current_mode == "neutron") {
            geometry_msgs::msg::PointStamped neutron_center;
            if(detection_manager_->detect("neutron", cloud, neutron_center)) {

                neutron_center = applyExponentialFilter(neutron_center, last_neutron_point_);

                // 发布中心点标记
                neutron_center.header.stamp = this->now();
                neutron_center.header.frame_id = "map";
                pub_neutron_center_->publish(neutron_center);

                json_coord_.set_from_coord(neutron_center.point.x, neutron_center.point.y, neutron_center.point.z);
                json_msg_.data = json_coord_.get_json_string();
                pub_center_json_->publish(json_msg_);

                RCLCPP_INFO(this->get_logger(),"neutron center detected:(%.3f, %.3f, %.3f)",
                            neutron_center.point.x, neutron_center.point.y, neutron_center.point.z);
            }
        }

        //仅使用锥形放射源检测
        else if(current_mode == "gamma") {
            geometry_msgs::msg::PointStamped gamma_center;
            if(detection_manager_->detect("gamma", cloud, gamma_center)) {
                gamma_center = applyExponentialFilter(gamma_center, last_gamma_point_);

                gamma_center.header.stamp = this->now();
                gamma_center.header.frame_id = "map";
                pub_gamma_center_->publish(gamma_center);

                json_coord_.set_from_coord(gamma_center.point.x, gamma_center.point.y, gamma_center.point.z);
                json_msg_.data = json_coord_.get_json_string();
                pub_center_json_->publish(json_msg_);

                RCLCPP_INFO(this->get_logger(),"gamma center detected:(%.3f, %.3f, %.3f)",
                            gamma_center.point.x, gamma_center.point.y, gamma_center.point.z);
            }
        }
    }

    // 指数移动平均滤波
    geometry_msgs::msg::PointStamped CenterDetector::applyExponentialFilter(
        const geometry_msgs::msg::PointStamped& raw_point,
        geometry_msgs::msg::Point& last_filtered) {

        geometry_msgs::msg::PointStamped filtered_point = raw_point;

        // 第一次检测或初始化
        if (last_filtered.x == 0 && last_filtered.y == 0 && last_filtered.z == 0) {
            last_filtered.x = raw_point.point.x;
            last_filtered.y = raw_point.point.y;
            last_filtered.z = raw_point.point.z;
            return filtered_point;
        }

        // 指数移动平均：新值 = alpha * 当前值 + (1-alpha) * 上一次滤波值
        filtered_point.point.x = alpha_ * raw_point.point.x + (1.0 - alpha_) * last_filtered.x;
        filtered_point.point.y = alpha_ * raw_point.point.y + (1.0 - alpha_) * last_filtered.y;
        filtered_point.point.z = alpha_ * raw_point.point.z + (1.0 - alpha_) * last_filtered.z;

        // 更新历史值
        last_filtered = filtered_point.point;

        return filtered_point;
    }
}

// --- 主函数修改 ---
int main(int argc, char** argv) {
    rclcpp::init(argc, argv);

    // 创建生命周期节点
    auto node = std::make_shared<center_detection::CenterDetector>();

    // 使用 MultiThreadedExecutor 以便正确处理生命周期
    rclcpp::executors::MultiThreadedExecutor executor;
    executor.add_node(node->get_node_base_interface());
    executor.spin();

    rclcpp::shutdown();
    return 0;
}